# -*- coding: utf-8 -*-
"""
@File    : Test.py
@Author  : LY
@Time    : 2021/5/11 20:43
"""
from FFT_Interpolation import line_cal, line_cal_fix, FFT_cal, FFT_interpolation_2
import numpy as np
import matplotlib.pyplot as plt
import random
from tqdm import tqdm

def dual_Gaussian(x, sig, n_mid=0, sigma=0.2):
    N_mid_1 = 640 - n_mid
    N_mid_2 = 640 + n_mid
    gaussian_1 = np.exp(-0.5 * ((x - N_mid_1) / (sigma * 1280 / 2)) ** 2)
    gaussian_2 = np.exp(-0.5 * ((x - N_mid_2) / (sigma * 1280 / 2)) ** 2)
    window = (gaussian_1 + gaussian_2)
    c = np.exp(-0.5 * ((x - 640) / (sigma * 1280 / 2)) ** 2 * (12 * n_mid / 1280)) * (1 - 2 * n_mid / 1280) ** 2
    data = ((sig - np.mean(sig)) * c + np.mean(sig)) * window
    return data


def Parabola_fitting_2(mk, Y, delta_f):  # 三点式抛物线拟合
    Y_mk_minus = Y[0]
    Y_mk = Y[1]
    Y_mk_plus = Y[2]
    Cr2 = (Y_mk_plus - Y_mk_minus) / (2 * Y_mk - Y_mk_minus - Y_mk_plus) / 2
    f_fit = (mk + Cr2) * delta_f
    return f_fit, Cr2


def FFT_calculate(signal, tau):  # 传参：信号与空间参数
    N = len(signal)  # 信号采样点数
    freq_axis = np.fft.fftfreq(N, tau)[0:(N + 1) // 2]  # 频率轴设置
    sig_fft = np.fft.fft(signal)[0:(N + 1) // 2]  # 快速傅里叶变换
    sig_fft_magnitude = np.abs(sig_fft) * 2 / N
    sig_fft_magnitude[0] = sig_fft_magnitude[0] / 2  # 频谱幅值
    sig_fft_phase = np.angle(sig_fft)  # 频谱相位
    return freq_axis, sig_fft_magnitude, sig_fft_phase  # 返参


def Peak_search(freq_axis, sig_fft_magnitude, sig_fft_phase, DC_bias):  # 寻峰
    N = len(freq_axis)
    freq_point_max_Vp = np.max(sig_fft_magnitude[DC_bias:N - 1])  # 最大峰值
    freq_max_point = np.argmax(sig_fft_magnitude[DC_bias:N - 1]) + DC_bias  # 最大峰值对应频率点
    freq_point = freq_axis[freq_max_point]
    phase_point = sig_fft_phase[freq_max_point]
    return freq_point_max_Vp, freq_point, phase_point, freq_max_point


pix_size = 5.3e-6  # d_p
pix_num = 1280  # N_p
screen_diameter = pix_num * pix_size

x = np.linspace(0, (pix_num - 1), pix_num)
tau0 = pix_size
fs = 1 / tau0
N = len(x)

f = 3e3 + 100
phi = 0
result = []
for i in tqdm(range(370)):
    sig = (1000 * np.cos(2 * np.pi * f * x * pix_size + phi) / 2 + 512)
    data = dual_Gaussian(x=x, sig=sig, n_mid=0, sigma=0.2)
    line_noise = np.array([np.random.normal(2.43, 2.6, 1280)])[0]
    data_noisy = data + line_noise
    data_noisy = np.array(data_noisy).round().astype(int)
    data_noisy = dual_Gaussian(x=x, sig=data_noisy, n_mid=0, sigma=0.4)
    # data_noisy[0:200] = np.zeros(200)
    # data_noisy[1000:] = np.zeros(280)
    # plt.plot(data_noisy)
    # plt.show()
    freq_estim_2, phase_estim_2, freqline, sig_magnitude, sig_phase, m_k_num, X_m_k, freq_for_phase = FFT_interpolation_2(
        sig=data_noisy, tau0=pix_size, zero_num=pix_num * 99, DC_num=300)
    result.append(freq_estim_2)
# print(freq_estim_2)
# plt.plot(result)
# plt.show()
print(np.std(result))


# zero_num = pix_num * 99
# zero_padd = np.zeros(int(zero_num))
# sig_padd = np.concatenate((data_noisy, zero_padd))
# freq_axis, sig_fft_magnitude, sig_fft_phase = FFT_calculate(signal=sig_padd, tau=tau0)
# freq_point_max_Vp, freq_point, phase_point, freq_max_point = Peak_search(freq_axis, sig_fft_magnitude, sig_fft_phase,
#                                                                          DC_bias=300)
# delta_f = fs / len(sig_padd)
# freq_fit, Cr2 = Parabola_fitting_2(freq_max_point, sig_fft_magnitude[freq_max_point - 1:freq_max_point + 2], delta_f)
# print(freq_fit)

# sig_yl = (1000 * np.cos(2 * np.pi * f * x * pix_size + phi) / 2 + 512) / 2
# line_noise = np.array([np.random.normal(2.43, 2.6, 1280)])[0]
#
# sig_lxl = 250 * np.cos(2 * np.pi * x * pix_size * f + phi) + 256
# noise = [random.gauss(2.43, 2.6) for m in range(pix_num)]
#
# sig_lxl = sig_lxl + noise
# sig_yl = sig_yl + line_noise
#
# plt.plot(sig_yl, 'k')
# plt.plot(sig_lxl, 'r')
# plt.show()
